using System;
using Sce.PlayStation.Core;
namespace Nyanimals
{
    /// <summary>
    /// Simple Axis angle class
    /// </summary>
    public class CAxisAngle
    {
        private Vector3 position;
        private Vector3 axis;
        private float angle;
        
        public Vector3 Position
        {
            get { return position;}
            set { position = value;}
        }
        public Vector3 Axis
        {
            get { return axis;}
            set { axis = value;}
        }
        public float Angle
        {
            get { return angle;}
            set { angle = value;}
        }
        /// <summary>
        /// Gets the values as a 4x4 transform matrix.
        /// </summary>
        /// <returns>
        /// The matrix.
        /// </returns>
        public Matrix4 GetMatrix()
        {
            Console.WriteLine("Untested Function Call: CAxisAngle::GetMatrix()");
            Matrix4 outMat = Matrix4.RotationAxis(axis, angle);
            outMat.AxisW = position;
            
            return outMat;
        }
        /// <summary>
        /// Initializes an empty instance of the <see cref="Nyanimals.AxisAngle"/> class.
        /// </summary>
        public CAxisAngle()
        {
            position = Vector3.Zero;
            axis = Vector3.UnitY;
            angle = 0;
        }
        /// <summary>
        /// Initializes a new instance of the <see cref="Nyanimals.AxisAngle"/> class from a transformation Matrix
        /// </summary>
        /// <param name='inMatrix'>
        /// Matrix.
        /// </param>
        public CAxisAngle(Matrix4 inMatrix)
        {      
            float epsilon = 0.001f;
            
            // save and toss any translation data
            Position = inMatrix.AxisW;
            inMatrix.ColumnW = Vector4.UnitW;
                       
            // EASY OUT: Check for 0 or 180 degrees
            // 00 x1 x2
            // x1 00 x3
            // x2 x3 00
            float x1 = inMatrix.M12 - inMatrix.M21;
            float x2 = inMatrix.M13 - inMatrix.M31;
            float x3 = inMatrix.M23 - inMatrix.M32;
            
            if(FMath.Abs(x1) < epsilon && FMath.Abs(x2) < epsilon && FMath.Abs(x3) < epsilon)
            {
                // check if angle is 0(identity)
                if(inMatrix.IsIdentity())
                {
                    // angle of 0 and arbituary axis
                    angle = 0.0f;
                    axis = new Vector3(1);
                    return;
                }
                
                // angle is at 180
                angle = FMath.PI;
                
                // handle special case of having angle of 180 by checking the largest diaginal
                float xx = (inMatrix.M11 + 1) / 2;
                float yy = (inMatrix.M22 + 1) / 2;
                float zz = (inMatrix.M33 + 1) / 2;
                float xy = (inMatrix.M12 + inMatrix.M21) / 4;
                float xz = (inMatrix.M13 + inMatrix.M31) / 4;
                float yz = (inMatrix.M23 + inMatrix.M32) / 4;
                axis = Vector3.Zero;
                
                // xx is largest in the diaginal
                if ((xx > yy) && (xx > zz)) 
                {
                    if (xx< epsilon) 
                    {
                        axis.X = 0;
                        axis.Y = 0.7071f;
                        axis.Z = 0.7071f;
                    } 
                    else 
                    {
                        axis.X = FMath.Sqrt(xx);
                        axis.Y = xy/axis.X;
                        axis.Z = xz/axis.X;
                    }
                } 
                // yy is largest in the diaginal
                else if (yy > zz) 
                {
                    if (yy< epsilon) 
                    {
                        axis.X = 0.7071f;
                        axis.Y = 0;
                        axis.Z = 0.7071f;
                    } 
                    else 
                    {
                        axis.Y = FMath.Sqrt(yy);
                        axis.X = xy/axis.Y;
                        axis.Z = yz/axis.Y;
                    }   
                } 
                // zz has to be larget if we are here...
                else 
                {
                    if (zz< epsilon) 
                    {
                        axis.X = 0.7071f;
                        axis.Y = 0.7071f;
                        axis.Z = 0;
                    } 
                    else 
                    {
                        axis.Z = FMath.Sqrt(zz);
                        axis.X = xz/axis.Z;
                        axis.Y = yz/axis.Z;
                    }
                }
                return; // we are returning an angle of 180                
            }
            
            // if we are here than compute as normal
            float denominator;
            float x, y, z;
            float d1 = x = inMatrix.M32 - inMatrix.M23;
            float d2 = y = inMatrix.M13 - inMatrix.M31;
            float d3 = z = inMatrix.M21 - inMatrix.M21;
            
            // get the axis
            d1 *= d1; d2 *= d2; d3 *= d3;
            denominator = FMath.Sqrt(d1 + d2 + d3);
            x /= denominator; y /= denominator; z /= denominator;            
            axis = new Vector3(x, y, z);
           
            // calculate the angle
            angle = inMatrix.M11 + inMatrix.M22 + inMatrix.M33 - 1;
            angle /= 2;
            angle = FMath.Cos(angle);
            
            return;
        }        
        /// <summary>
        /// Initializes a new instance of the <see cref="Nyanimals.AxisAngle"/> class with passed in values
        /// </summary>
        /// <param name='position'>
        /// Position.
        /// </param>
        /// <param name='axis'>
        /// Axis.
        /// </param>
        /// <param name='angle'>
        /// Angle.
        /// </param>
        public CAxisAngle(Vector3 position, Vector3 axis, float angle)
        {
            this.position = position;
            this.axis = axis;
            this.angle = angle;
        }
        /// <summary>
        /// Initializes a new instance of the <see cref="Nyanimals.AxisAngle"/> class with passed in values
        /// </summary>
        /// <param name='axis'>
        /// Axis.
        /// </param>
        /// <param name='angle'>
        /// Angle.
        /// </param>
        public CAxisAngle(Vector3 axis, float angle)
        {
            this.position = Vector3.Zero;
            this.axis = axis;
            this.angle = angle;
        }
    }
}

